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Abstract  In  cooperative  localization  a  group  of  robots 
exchange  relative  position  measurements  from  their  ex¬ 
teroceptive  sensors  and  their  motion  information  from 
interoceptive  sensors  to  collectively  estimate  their  po¬ 
sition  and  heading.  For  the  localization  errors  to  be 
bounded,  it  is  required  that  the  system  be  observable, 
independent  of  the  estimation  technique  being  used. 
In  this  paper,  we  develop  a  test-bed  of  three  ground 
robots,  which  are  equipped  with  wheel  encoders  and 
omnidirectional  cameras,  to  implement  the  bearing-only 
cooperative  localization.  The  simulation  and  experimen¬ 
tal  results  validate  the  observability  conditions,  derived 
in  [1],  for  the  complete  observability  of  the  bearing-only 
cooperative  localization  problem. 
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1  Introduction 

In  cooperative  localization  a  group  of  robots  exchange 
relative  position  measurements  from  their  exteroceptive 
sensors  (e.g.,  camera,  laser,  etc.)  and  their  motion  in¬ 
formation  (velocity  and  turn  rate)  from  interoceptive 
sensors  (e.g.,  IMU,  encoders,  etc.)  to  collectively  esti¬ 
mate  their  states.  Cooperative  localization  has  been  an 
active  area  of  research  (e.g.,  [2-9])  because  it  provides 
several  potential  advantages,  including  increased  local¬ 
ization  accuracy,  sensor  coverage,  robustness,  efficiency, 
and  flexibility. 

Recently,  estimation  algorithms  such  as  the  Extended 
Kalman  Filter  (EKF)  [10],  Minimum  Mean  Square  Es¬ 
timator  (MMSE)  [3],  Maximum  Likelihood  Estimation 
(MLE)  [11],  Particle  Filter  [12],  and  Maximum  A  Pos¬ 
teriori  (MAP)  [13],  have  been  used  to  solve  the  co¬ 
operative  localization  problem.  These  algorithms  can 
be  used  either  in  centralized  [6]  or  decentralized  man¬ 
ner  [3,10,13].  For  the  localization  errors  to  be  bounded, 
it  is  required  that  the  system  be  observable,  indepen¬ 
dent  of  the  estimation  technique  being  used. 

Several  authors  have  carried  out  observability  anal¬ 
ysis  of  the  cooperative  localization  problem.  Initial  re¬ 
sults  regarding  the  observability  of  cooperative  local¬ 
ization  were  reported  by  Roumeliotis  and  Bekey  [10]. 
They  used  linear  observability  analysis  to  show  that 
the  states  of  the  robots  performing  cooperative  localiza¬ 
tion  are  unobservable,  but  can  be  made  observable  by 
providing  global  positioning  information  to  one  of  the 
robots.  In  [10]  it  was  assumed  that  the  absolute  vehicle 
heading  is  measured  directly  and  does  not  need  to  be 
estimated.  Furthermore,  linear  approximation  of  a  non¬ 
linear  system  can  provide  different  structural  properties 
regarding  the  observability  [14,15].  Martinelli  et  al.  [16] 
investigates  the  nonlinear  observability  of  cooperative 
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localization  for  two  robots  without  heading  measure¬ 
ments.  They  compared  the  observability  properties  of 
range  and  bearing  measurements  and  showed  that  with 
either  type  of  measurement,  the  maximum  rank  of  the 
observability  matrix  is  three,  i.e. ,  not  fully  observable. 
The  analysis  in  [16]  shows  that  relative  bearing  is  the 
best  observation  between  the  robots.  The  part  of  the 
system  which  is  observable  is  in  general  larger  than  for 
the  other  relative  observations  (relative  distance  and 
relative  orientation).  Accordingly,  [16]  uses  polar  coor¬ 
dinates  for  the  observability  analysis. 

In  our  previous  work  [1],  we  have  extended  the  ob¬ 
servability  analysis  presented  in  [16]  from  2  to  n  robots, 
with  bearing-only  measurements.  The  extension  for  n  > 
2  is  not  obvious  because  of  the  dynamically  changing  set 
of  n(n  —  l)/2  different  relative  bearing  measurements 
leading  to  2nV~1)/2  possible  configurations.  Further¬ 
more,  since  the  robot  states  in  [16]  are  not  observable 
with  respect  to  a  global  reference  frame,  and  since  it 
has  been  shown  that  two  landmarks  are  needed  for  the 
observability  of  a  single  vehicle  [17-19],  in  [1]  we  have 
derived  the  number  of  landmarks  needed  for  full  observ¬ 
ability  of  a  group  of  n  robots  performing  cooperative 
localization.  In  contrast  to  [10],  we  have  assumed  that 
the  heading  of  each  robot  is  not  directly  measured  but 
must  be  estimated. 

In  [1] ,  we  have  used  the  Relative  Position  Measure¬ 
ment  Graph  (RPMG)  to  represent  a  group  of  robots. 
The  nodes  of  an  RPMG  represent  vehicle  states  and  the 
edges  represent  bearing  measurements  between  nodes. 
We  have  established  a  relationship  between  the  graph 
properties  of  the  RPMG  and  the  rank  of  the  system  ob¬ 
servability  matrix.  We  have  proved  that  for  a  connected 
RPMG,  the  observability  matrix  for  a  team  of  n  robots, 
which  has  size  3 n  x  3n  will  have  rank  3(n  —  1).  We  also 
derive  conditions  under  which  landmarks  observed  by 
a  subset  of  robots  enable  the  system  to  become  fully 
observable. 


Robot 


Landmark 

Measurement 


Fig.  1  Relative  position  measurement  graph  (RPMG).  The 
nodes  of  an  RPMG  represent  vehicle  states  and  the  edges 
represent  bearing  measurements  between  nodes. 


2  Bearing-only  Cooperative  Localization 

Consider  n  robots  moving  in  a  horizontal  plane  per¬ 
forming  cooperative  localization.  We  can  write  the  equa¬ 
tions  of  motion  for  the  ith  robot  as 

(V  cos ipi \ 

V  sin  'ipi  \  ,  (1) 

where  Xi  =  [a:,  yi  ipi]T  G  R3  is  the  robot  state,  includ¬ 
ing  robot  location  (&»,  yi)  and  robot  heading  ipi,  and 
Ui  =  \Vi,Wi]T  is  the  control  input  vector.  We  assume 
that  onboard  introspective  sensors  (e.g.,  encoders)  mea¬ 
sure  the  linear  speed  V,  and  angular  speed  Wj  of  the 
robot.  Without  loss  of  generality,  we  assume  that  robots 
cannot  move  backward  (Vi  >0,  i  =  1  •  •  •  n).  Each  ve¬ 
hicle  has  an  exteroceptive  sensor  to  measure  relative 
bearing  to  other  vehicles  and  known  landmarks  that 
are  in  the  field-of-view  of  the  sensor.  Relative  bearing 
from  the  ith  robot  to  the  jth  robot  or  landmark  can  be 
written  as 

i lij  =  tan-1  f  ^  -  ipi.  (2) 

V  Xj  Xi  J 


In  this  paper,  we  develop  a  test-bed  of  three  ground 
robots,  which  are  equipped  with  wheel  encoders  and 
omnidirectional  cameras,  to  implement  the  bearing-only 
localization.  We  present  simulation  and  experimental 
results  to  validate  the  observability  conditions  [1]  for 
the  complete  observability  of  the  bearing-only  cooper¬ 
ative  localization  problem. 

The  paper  is  organized  as  follows.  In  Section  2  we 
describe  bearing-only  cooperative  localization  and  for¬ 
mulate  the  problem.  In  Section  3  we  summarize  the 
observability  analysis  results  obtained  in  [1].  The  sim¬ 
ulation  and  experimental  results  are  presented  in  Sec¬ 
tion  4.  In  Section  5  we  give  our  conclusions. 


For  cooperative  localization,  each  robot  exchanges 
its  local  sensor  measurements  (velocity,  turn  rate,  and 
bearing  to  landmarks  and  other  robots)  with  their  neigh¬ 
bors.  Let  N,M  be  the  set  of  neighbors  for  which  robot 
i  can  obtain  bearing  measurements,  and  let  Nf  be  the 
set  of  neighbors  with  which  robots  i  can  communicate. 
In  this  paper,  we  assume  that  =  Nf  and  we  will 
therefore  denote  the  set  of  neighbors  as  N.t .  To  represent 
the  connection  topology  of  the  robots  we  use  a  relative 
position  measurement  graph  (RPMG)  [20]  which  is  de¬ 
fined  as  follows. 

Definition  1  An  RPMG  for  n  robots  performing  co¬ 
operative  localization  with  l  different  known  landmarks 
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is  a  directed  graph  Gln  =  {Vn,i,£n,i},  where  Vnj  = 
{1,  •  •  •  ,  n,  n  +  1,  •  •  •  ,  n  +  1}  is  the  node  set  consisting 
of  n  robot  nodes  and  l  landmark  nodes  and  £n,i(t)  C 
{V„j0xV„,z}  =  {r]ij},  *£{!,••■  ,n},  j  €  {1,  -  -  -  ,n,n+ 
1,  •  •  •  ,  n+l}  is  the  edge  set  representing  the  availability 
of  a  relative  bearing  measurement.  We  use  m  to  denote 
the  number  of  edges  in  the  RPMG.  An  example  RPMG 
(G3  with  m  =  7)  is  shown  in  Fig.  1. 

Additionally,  without  loss  of  generality  we  assume  that 
robots  maintain  a  safe  distance  from  each  other  and 
from  landmarks,  i.e. ,  Rij  >  0,  Vi ,  j  =  1,  •  •  •  n  and  land¬ 
marks  Rik  >0,  Vi  =  1,  ■  ■  •  n;  k  =  1,  ■  ■  ■  ,1. 


are  given  below 


Fi 


Bi 


T, 


dfj 
1  dXi 


|  Xi=Xi(k) 


1  0  —V{rs  sin  ip(k) 
0  1  ViTs  cos  ip(k) 
0  0  1 


rp  dfi ; 

sdUl 


|  Ui=Ui{k) 


Ts  cos  i/’fe  0 
Ts  sin  il>k  0 
0  Ts 


(8) 

(9) 


and  Qi  ( k )  =  ^  <J^i  2  ^  >  where  aVi  and  <7Ui  are  the 

standard  deviation  in  velocity  input  and  turn  rate  input 
respectively. 

The  measurement  update  step  is  given  as 


2.1  Cooperative  Localization  implementation 

The  objective  of  the  cooperative  localization  is  to  esti¬ 
mate  the  combined  state  X(k)  =  [Ad(fc),  •  •  •  ,  Xn(k)]T . 
We  use  an  extended  information  filter(EIF)  to  imple¬ 
ment  the  bearing-only  cooperative  localization.  In  the 
information  filter  instead  of  state  X  and  covariance 
P{k)  the  information  vector  y(k)  and  information  ma¬ 
trix  Y (k)  is  updated.  The  information  matrix  and  in¬ 
formation  vector  can  be  computed  as 

Y(k)  =  P(k)-1  (3) 

m  =  Y(k)X(k)  (4) 

Similar  to  an  extended  kalman  filter  (EKF)  the  EIF 
has  two  steps.  The  first  is  the  prediction  step,  which  is 
given  below. 


Y(k  +  l\k  +  1)  =Y{k  +  l\k)  +  '^2Hij(k)rarit2.Hij(k) 

y(k  +  1|  k  +  1)  =  y(k  +  1|  k)  ■  ■  ■ 

+  ^2  Bij{k)T R^1  (fiij  +  HijX(k  +  l|fe)) 

The  scalar  fiij  represents  the  innovation 

A Hj  =  Vij  ~  hij(x(k  +  l|fc))  (10) 

and  am ,  is  standard  deviation  of  the  noise  in  the  bearing 
measurement.  The  row  vector  Hij  is  the  measurement 
jacobian 

■  • 

Hij{k)  =  ~oxl lx=x^-  (11) 

The  EIF  is  dual  of  the  EKF  and  the  EKF  is  a  quasi¬ 
local  asymptotic  observer  for  nonlinear  systems  and  its 
convergence  and  boundedness  are  achieved  when  the 
system  is  fully  observable  [21]. 


Y(k  +  l|jfc)  =  (F(k)Y  (k\k)~1F(k)T  +  B{k)Q{k)B{k)T)~1 

(5) 

y(k  +  l|fc)  =  Y(k  +  l\k)X(k  +  l\k)  (6) 

X{k  +  l|fc)  =  X{k\k)  +  TJ(X(k\k),  u(k))  (7) 


2.2  Lie  Derivatives  and  Nonlinear  Observability 

To  determine  the  observability  of  the  entire  system  rep¬ 
resented  by  the  RPMG  we  use  the  nonlinear  observabil¬ 
ity  rank  criteria  developed  by  Hermann  and  Krener  [22] 
which  is  summarized  in  the  next  paragraph. 

Consider  a  system  model  with  the  following  form 


where  Fj- 


Q(k )  = 


(F1  0  •••  0  \ 

0  f2  •••  0 

:  •••  0 
VO  0  Fj 
( Qi{k )  0  0 


,  B(k)  = 


(Bi 


\Bn 


,  and 


is  covariance  of  noise  in 


0  '.0 

V  0  0  Qn(k), 

the  control  input.  The  matrix  Ft  and  Bi  are  the  system 
jacobian  with  respect  to  state  Xt  and  control  Uj.  which 


X  =  f(X,u)  =  [f?(X1,u1),---  ,fJ(Xn,un)]T 
■  y  =  h(X1Z)  =  [hJ{X1Z)---hTm{X1Z)}T 

(12) 

where  X  =  [Xj Xj  ■  ■  ■  AT^]t  £  K3"  is  the  state  of  the 
system,  Z  =  [Zj Zj  ■  ■  ■  Zj]T  €  K2i  is  the  position  vec¬ 
tor  of  all  landmarks,  =  [ Xi  y,]T  is  the  position  vector 
of  ith  landmark,  hi  :  R3"  x  R2i  i— >  R  is  the  measurement 
model  of  the  ith  sensor,  u  £  A  C  R2"  is  the  control  in¬ 
put  vector,  and  g  :  R3n  x  A  R3n.  We  consider  the 
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special  case  where  the  process  function  g  can  be  sepa¬ 
rated  into  a  summation  of  independent  functions,  each 
one  excited  by  a  different  component  of  the  control  in¬ 
put  vector,  i.e. , 

X  —  f(X,u)  =  fVlVi  +  /WlW  1  +  •  •  •  +  fvnVn  +  funW>n 

(13) 

The  zerotlr-order  Lie  derivative  of  any  (scalar)  function 
is  the  function  itself,  i.e.,  L0hk(X,Z)  =  hk(X,Z).  The 
first-order  Lie  derivative  of  function  hk(X,Z)  with  re¬ 
spect  to  fVi  is  defined  as 

L)vh  =  VL°h-fVi  (14) 

V  represents  the  gradient  operator,  and  •  denotes  the 
vector  inner  product.  Considering  that  Lj,  hk(X,Z)  is 
a  scalar  function  itself,  the  second-order  Lie  derivative 
of  hk(X ,  Z)  with  respect  to  fVi  is 

L}Vifvh  =  XL)vh-  fVi.  (15) 

Higher  order  Lie  derivatives  are  computed  similarly. 
Additionally,  it  is  possible  to  define  mixed  Lie  deriva¬ 
tives,  i.e.,  with  respect  to  different  functions  of  the  pro¬ 
cess  model.  For  example,  the  second-order  Lie  deriva¬ 
tive  of  hk  with  respect  to  fv . ,  given  its  first  derivative 
with  respect  to  fVi,  is 

(16) 

Based  on  the  preceding  expressions  for  the  Lie  deriva¬ 
tives  the  observability  matrix  is  defined  as  the  matrix 
with  rows 

°  =  {vb.„-  <17) 


3  Graph-based  Observability  Analysis 

In  this  section,  we  summarize  the  conditions  for  the  ob¬ 
servability  of  the  graph  Gln  derived  in  [1],  In  a  graph 
Gln  there  are  two  types  of  edges:  an  edge  between  two 
robots,  and  an  edge  between  a  robot  and  a  landmark. 
The  linearly  independent  rows  of  the  observability  sub¬ 
matrix  of  an  edge  serve  as  building  block  for  the  ob¬ 
servability  conditions  for  the  graph  Gln. 

The  observability  matrix  of  an  edge  between  two 
robots  can  be  written  using  gradients  of  Lie  derivatives 
(detailed  derivation  is  given  in  [1])  up  to  second-order 
as 


— Ay  ij 

AXij 

Ayij 

Axij 

0 

Stpj 

-ctpi 

Jt 

-Slpi 

ctpi 

0 

si’j 

Clpj 

0 

stpj 

-Ctpj 

—2  Axij 

-2 Ayij 

0 

2  Axjj 

2 Ayij 

0 

0 

0 

T ip 

0 

0 

J'ljj 

ctpi 

Slpi 

Jr 

-Ctpi 

Slpi 

0 

-Clpj 

-Stpj 

0 

Ctpj 

Stpj 

(18) 

The  next  lemma  provides  conditions  for  the  max¬ 
imum  rank  of  the  observability  matrix  of  an  edge  be¬ 
tween  two  robots. 

Lemma  1  (  [1])  The  rank  of  Oij  given  by  (18)  (edge 
between  two  robots)  is  three  if 

1.  Vi  >  0, 

2.  Vj  >  0, 

3.  the  ith  robot,  which  is  measuring  the  bearing,  does 
not  move  along  the  line  joining  the  two  robots, 

4 ■  the  jth  robot  does  not  move  perpendicular  to  the  line 
joining  the  two  robots. 

Similarly,  the  observability  matrix  corresponding  to  an 
edge  between  a  robot  and  a  landmark,  using  the  gradi¬ 
ents  of  Lie  derivatives  up  to  second  order,  as 


—  Ay  ik 

AXik 

-R% 

where  i.j  =  1,  •  •  •  ,  n;  k  =  1,  •  •  •  ,  m;p  £  N.  The  impor¬ 

Oik  = 

Stpi 

—C'lpi 

j+ 

tant  role  of  this  matrix  in  the  observability  analysis  of 

2Axik 

2  Ayik 

0 

a  nonlinear  system  is  demonstrated  by  Theorem  1. 

Ctpi 

S^i 

j- 

Theorem  1  A  system  is  locally  weakly  observable  if  its 
observability  matrix  whose  rows  are  given  in  (17)  has 
full  rank,  e.g.,  in  our  case  rank(O)  =  3 n. 

Additionally,  we  assume  that  the  robot  sensors  have 
limited  sensor  range  psensor  and  limited  field  of  view. 
Therefore,  agents  can  only  measure  the  bearing  of  those 
robots  and  landmarks  that  are  located  within  the  foot¬ 
print  of  the  sensor.  Therefore,  the  graph  Gln  will  likely 
have  a  time  varying  topology. 


where  J+  =  Axikctpi  +  Ayikstpi  and  J  =  Ayikcipi  - 
AxikSifi. 

Lemma  2  (  [1])  The  rank  Olk  given  by  (19)  (edge  be¬ 
tween  a  robot  and  a  landmark)  is  two  if 

1.  Vi  >  0, 

2.  the  robot  does  not  move  along  the  line  joining  the 
robot  and  the  landmark. 

Next,  we  define  a  proper  RPMG. 
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Definition  2  An  RPMG  Gln  (Definition  1)  is  called 
a  proper  RPMG  if  all  the  edges  between  robot  nodes 
satisfy  the  conditions  of  Lemma  1  and  all  the  edges 
between  robots  and  landmarks  satisfy  Lemma  2. 

Next,  theorem,  which  is  proved  in  [1],  summarize  the 
observability  conditions  for  a  proper  RPMG  G°  with¬ 
out  any  landmarks. 

Theorem  2  (  [1])  If  the  graph  G°  is  a  proper  con¬ 
nected  RPMG  then  the  rank  of  the  associated  observ¬ 
ability  matrix  is  3  (n  —  1). 

It  should  noted  that  the  maximum  rank  of  the  observ¬ 
ability  matrix  of  RPMG  G°  without  any  landmarks  is 
less  then  3n,  therefore,  the  system  in  not  completely  ob¬ 
servable.  Next  theorem  provides  the  conditions  for  the 
complete  observability  of  the  system  in  the  presence  of 
known  landmarks. 

Theorem  3  (  [1])  Given  a  proper  RPMG  Gln,  if  for 
each  robot  there  exists  a  path  to  at  least  two  landmarks 
and  the  robot  and  two  landmarks  are  not  on  the  same 
line  (i.e.,  rjn  ^  ??i2,V  i  =  l,-—  ,n)  then  the  system  is 
completely  observable,  i.e.,  the  rank  of  the  observability 
matrix  is  3 n. 

An  alternative  method  for  obtaining  complete  observ¬ 
ability  without  landmarks  is  derived  in  the  nest  theo¬ 
rem. 

Theorem  4  Given  a  RPMG  G° ,  if  it  is  proper,  con¬ 
nected,  and  one  of  the  robot  has  its  position  and  heading 
measurement  from  GPS  then  the  system  is  completely 
observable,  i.e.,  the  rank  of  the  observability  matrix  is 
3  n. 

Proof  If  RPMG  G°  is  proper  and  connected  then  from 
Theorem  2  there  are  3(n—  1)  linearly  independent  rows 
in  the  observability  matrix  and  if  one  of  the  robot  mea¬ 
sures  its  position  and  heading  directly  from  a  GPS  re¬ 
ceiver  then  three  linearly  independent  rows,  which  are 
linearly  independent  to  the  existing  3(n  —  1)  rows,  are 
added.  Therefore,  the  rank  of  the  observability  matrix 
becomes  3 n  and  system  becomes  completely  observable. 


4  Results 

In  this  section,  we  present  simulation  and  hardware  re¬ 
sults  to  validate  the  observability  conditions  discussed 
in  the  previous  section. 


4.1  Simulation  Results 

The  simulation  environment  consists  of  five  homoge¬ 
nous  ground  robots  and  two  landmarks.  Each  robot  is 
equipped  with  an  encoder,  which  provides  the  robot’s 
linear  and  rotation  velocities,  and  a  omini-directional 
camera,  which  measure  bearing  from  other  robots  and 
landmarks  in  the  sensor  range  Rsensor •  We  assume  that 
the  noise  in  encoder  and  camera  measurement  are  Gaus¬ 
sian.  Following  are  the  different  simulation  parameters 
of  a  robot  used  in  the  simulation  results  presented  in 
this  paper. 

—  Sensing  range  of  the  omni-directional  camera  ( Rsensor  = 
120?n). 

—  Linear  velocity  of  the  robot  (V  =  5  m/s). 

—  Sampling  time  period  Ts  =  0.1  s. 

—  Initial  pose  uncertainty  (ax o  ay o  cq/,0]  =  [5m  5 m  0.2rad]) 

—  Standard  deviations  of  process  noise  in  encoder  [av  au]T 
[0.2m/s  0.2 rad/s]T. 

—  Standard  deviation  of  measurement  noise  aVij  = 

0.1  rad. 

To  numerically  prove  the  Theorem  3,  we  define  a 
metric  for  the  existence  of  a  path  between  two  nodes 
in  the  RPMG.  For  this,  we  make  use  of  the  ideas  of 
k-connectivity  [23]. 

The  adjacency  matrix  of  a  proper  RPMG  can  be 
defined  as  A  =  [al3\  G  #("+0  ><("+*)  where 

1  if  sin T]ij(RSensor  -  Rij )  >  o  and  i  G  [l,n] 

0  otherwise 

(20) 

The  k-connectivity  matrix  is  defined  as 

Gk  =  In+i  +  A  +  A2  +  •••-)-  Ak  (21) 

and  the  entry  Ck{i,  j )  can  be  interpreted  as  the  number 
of  paths  of  k-hops  or  less  that  connect  node  i  to  node 
j.  Next  we  define  a  metric  /3(G2)  for  RPMG  with  two 
landmarks  which  summarizes  all  of  the  conditions  in 
Theorem  3. 

d!G2!  =  /  1  ^  Hi=i,nCn(i,n  +  l)Cn(i,n  +  2)  >  0 
n  \  0  otherwise 

(22) 

From  Theorem  3  we  can  say  that  if  /3(G2)  =  1  then 
the  system  is  completely  observable.  In  Fig.  2,  we  plot 
the  smallest  singular  value  of  the  actual  nonlinear  ob¬ 
servability  matrix  and  /3(Gg).  It  can  be  seen  that  when 
/3(G§)  =  1  then  each  robot  node  in  RPMG  has  path 
to  two  landmarks  and  the  system  is  completely  observ¬ 
able.  On  the  other  hand,  when  /3(G§)  =  0  then  all  of 
the  robots  in  the  do  not  have  path  to  two  landmarks 
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Fig.  2  Connectivity  vs  Observability:  The  dashed  red  curve 
represents  the  connectivity,  of  a  RPMG  with  five  robots  and 
two  landmarks,  as  defined  in  (22).  The  solid  blue  curve  repre¬ 
sents  the  smallest  singular  value  of  the  nonlinear  observability 
matrix  as  defined  in  (17).  When  /3(G§)  =  1  then  each  robot 
node  in  RPMG  has  path  to  two  landmarks  and  the  system  is 
completely  observable.  On  the  other  hand,  when  /3(G§)  =  0 
then  all  of  the  robots  in  the  do  not  have  path  to  two  land¬ 
marks  and  system  is  unobservable. 


Fig.  3  Initial  topology  for  bearing-only  cooperative  local¬ 
ization.  The  black  circles  represent  the  initial  position  uncer¬ 
tainty  (3cr).  The  black  diamonds  represent  the  initial  position 
estimates  of  robots.  The  red  circles  are  the  true  initial  posi¬ 
tions  of  the  robots.  The  dashed  blue  line  represents  an  edge 
(bearing  measurement)  between  two  nodes.  Two  numbered 
squares  are  the  two  known  landmarks. 


Fig.  4  True  trajectories  of  all  of  the  five  robots.  The  tra¬ 
jectories  are  generated  by  following  way-points,  and  the  way- 
points  are  chosen  such  that  the  RPMG  G§  remains  connected. 


and  system  is  unobservable.  This  numerically  proves 
the  Theorem  3. 

We  run  the  bearing-only  cooperative  localization  al¬ 
gorithm  for  four  different  cases.  In  the  first  case  the 
measurements  from  the  landmarks  are  not  used  for  esti¬ 
mation.  In  the  second  case  bearing  measurements  from 
only  one  landmark  are  considered,  and  in  the  third  case 
bearing  measurements  from  two  landmarks  are  consid¬ 
ered.  Finally,  we  do  not  consider  any  landmarks,  how¬ 
ever,  the  absolute  position  and  heading  of  a  robot  from 
a  GPS  receiver  are  fused  with  inter  robot  bearing  mea¬ 
surements  to  perform  the  cooperative  localization.  The 
Fig.  3  shows  the  initial  topology  of  the  RPMG  with 
five  robots  and  two  landmarks.  Initially,  the  RPMG  is 
connected  and  the  waypoints  for  all  of  the  five  robots 
are  chosen  such  that  the  RPMG  is  remains  connected. 
The  actual  trajectories  of  all  of  the  five  robots  is  shown 
in  Fig.  4,  and  the  Fig  5  shows  the  true  and  estimated 
trajectories  of  the  second  robot,  for  all  of  the  four  cases. 
It  can  be  seen  that  estimated  trajectories  for  CL  with 
two  landmarks  and  GPS  (first  robot)  are  closest  to  the 
true  trajectory.  The  comparison  for  position  error  (sec¬ 
ond  robot)  for  all  the  four  cases  is  shown  in  Fig  6  and 
Fig.  7. 

Fig  8,  Fig.  9,  and  Fig.  10  show  the  plots  for  the 
second  robot’s  estimated  uncertainty  (3cr)  in  X ,  Y .  and 
if)  respectively  for  all  of  the  four  cases.  It  can  be  seen 
that  the  uncertainty  for  two  landmarks  and  GPS  case  is 
lower  then  the  uncertainty  related  to  no  landmark  and 
one  landmark.  This  is  because  with  two  landmarks  or 
absolute  position  and  heading  measurement  of  a  robot 
from  GPS  the  system  is  observable. 
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Fig.  5  The  second  robot  trajectories:(l)  The  true  trajectory 
is  represented  by  the  dashed  curve.  (2)  The  estimated  trajec¬ 
tory  using  CL  with  no  landmarks  is  represented  by  solid  curve 
with  circles. (3)  The  estimated  trajectory  using  CL  with  only 
one  landmarks  is  represented  by  the  solid  curve  with  squares. 
(4)The  estimated  trajectory  using  CL  with  two  landmarks  is 
represented  by  the  solid  curve  with  stars.  (5)  The  estimated 
trajectory  using  CL,  when  Robot  l’s  position  and  attitude  is 
measured  using  a  GPS,  is  represented  by  the  solid  curve  with 
diamonds. 


Fig.  6  The  comparison  of  second  robot’s  error  in  X  direc¬ 
tion.  This  figure  shows  the  plots  of  error  in  X  direction  with 
no  landmark,  one  landmark,  two  landmark,  and  with  GPS 
measurements  of  the  Robot  1.  The  error  with  no  landmark 
and  one  landmark  is  higher  than  error  with  two  landmarks 
and  GPS  because  in  last  two  case  the  system  is  observable. 


Fig.  7  The  comparison  of  second  robot’s  error  in  Y  direc¬ 
tion.  This  figure  shows  the  plots  of  error  in  Y  direction  with 
no  landmark,  one  landmark,  two  landmark,  and  with  GPS 
measurements  of  the  Robot  1.  The  error  with  no  landmark 
and  one  landmark  is  higher  than  error  with  two  landmarks 
and  GPS  because  in  last  two  case  the  system  is  observable. 


Fig.  8  The  comparison  of  second  robot’s  estimation 
uncertainty^^)  in  X  direction.  This  figure  shows  the  plots 
of  error  covariance  in  X  direction  with  no  landmark,  one 
landmark,  two  landmark,  and  with  GPS  measurements  of  the 
Robot  1.  The  error  with  no  landmark  and  one  landmark  is 
higher  than  error  with  two  landmarks  and  GPS  because  in 
last  two  case  the  system  is  observable. 

4.2  Experimental  Results 

For  experimental  validation  we  use  three  stinger  robots 
(see  Fig.  11)  with  serializer  controller.  For  turn  rate 
and  velocity  measurement  wheel  encoders  are  used.  For 
bearing  measurement  we  use  an  omnidirectional  camera 
and  an  EEE  pc  for  onboard  processing  and  communi- 
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Fig.  9  The  comparison  of  second  robot’s  estimation  uncer¬ 
tainty  (3tJy)  in  Y  direction.  This  figure  shows  the  plots  of 
error  covariance  in  Y  direction  with  no  landmark,  one  land¬ 
mark,  two  landmark,  and  with  GPS  measurements  of  the 
Robot  1.  The  error  with  no  landmark  and  one  landmark  is 
higher  than  error  with  two  landmarks  and  GPS  because  in 
last  two  case  the  system  is  observable. 


Fig.  10  The  comparison  of  second  robot’s  estimation  uncer¬ 
tainty  (3<Tpsi)  in  heading.  This  figure  shows  the  plots  of  error 
covariance  in  ip  direction  with  no  landmark,  one  landmark, 
two  landmark,  and  with  GPS  measurements  of  the  Robot  1. 
The  error  with  no  landmark  and  one  landmark  is  higher  than 
error  with  two  landmarks  and  GPS  because  in  last  two  case 
the  system  is  observable. 

cation.  The  experimental  setup  is  shown  in  Fig.  12.  It 
consist  of  three  robots  with  different  color  (green,  blue, 
and  orange)  for  their  identification.  The  snapshot  of  the 
experimental  area  taken  from  the  omnidirectional  cam¬ 
era  on  the  orange  robot  is  shown  in  Fig.  13.  The  robots 
communicate  with  each  other  on  a  wireless  network  us¬ 
ing  a  router.  We  use  an  overhead  camera  to  obtain  the 


true  robot  states  and  compare  the  estimated  states.  We 
use  color  segmentation  to  find  the  bearing  of  the  robots 
and  landmarks  which  are  in  the  image  plane  of  a  om¬ 
nidirectional  camera. 

To  do  cooperative  localization,  the  robots  needed 
a  reliable  method  of  exchanging  information  with  one 
another.  To  meet  this  need  we  designed  a  software  sys¬ 
tem  that  enabled  multiple  agents  to  discover  and  re¬ 
liably  communicate  with  one  other  over  a  small  local 
area  network.  We  called  this  system  the  Agent  Manage¬ 
ment  System.  In  the  Agent  Management  System  each 
robot  was  represented  by  a  single  software  construct  we 
called  a  Software  Agent.  The  Software  Agent  was  a  sin¬ 
gle  process,  run  on  the  robot’s  onboard  computer  that 
concurrently  managed  the  robot’s  processing  and  com¬ 
munication.  There  were  two  core  elements  that  enabled 
the  Software  Agents  to  communicate  reliably.  The  first 
element  was  peer  discovery.  The  second  was  message 
handling.  Peer  discovery  enabled  the  Software  Agents 
to  find  each  other  maintain  an  active  directory  of  all 
other  agents  that  it  could  communicate  with.  As  soon 
as  a  Software  Agent  process  started,  it  began  listening 
for  and  periodically  broadcasting  a  short  discovery  mes¬ 
sage  across  the  local  area  network.  When  another  Soft¬ 
ware  Agent  heard  one  of  these  messages,  it  sent  back  a 
reply  informing  the  sending  agent  its  contact  informa¬ 
tion  (an  IP  address  and  port  number).  Within  a  short 
period  of  time  all  software  agents  had  a  list  of  every  ac¬ 
tive  agent  on  the  network.  Each  agent  would  continue 
pinging  each  other  (at  a  less  frequent  interval)  if  the 
neighboring  robot  failed  to  reply  to  a  certain  number 
of  pings,  it  was  removed  from  the  list  of  available  robot’s 
to  communicate  with.  With  a  reliable  list  of  neighbor¬ 
ing  agents  to  communicate  with,  the  agents  were  able  to 
start  sending  information  back  and  forth  between  each 
other.  Each  message  was  serialized  and  sent  as  a  single 
UDP  packet.  When  received,  the  Software  Agent  would 
put  the  messages  in  a  single  message  queue  that  could 
be  read  from  and  processed.  We  elected  to  use  the  UDP 
protocol  because  we  needed  small  rapid  measurements 
from  each  robot  in  order  to  make  good  cooperative  esti¬ 
mates.  Using  the  Agent  Management  System,  we  were 
able  to  design,  test,  and  record  our  experiment  for  co¬ 
operative  navigation  and  control. 

Following  parameters  are  used  for  the  experiment 

—  Linear  velocity  of  the  robot  (V  =  0.2  m/s). 

—  Sampling  time  period  Ts  =  0.1  s. 

—  Standard  deviations  of  process  noise  in  encoder  [cr^ 
[0.08m/s  0.12rad/s]T. 

—  Standard  deviation  of  measurement  noise  aVi .  = 
O.lSrad. 

Fig.  14  shows  the  trajectory  of  all  of  the  three  robots, 
which  are  computed  (1)  using  only  encoders,  (2)using 
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Fig.  11  The  Stinger  robot.  This  robot  is  equipped  with  a 
serialize  controller,  two  wheel  encoders,  an  omnidirectional 
camera,  and  Asus  EEE  computer. 


bearing-only  cooperative  localization,  and  (3)using  the 
overhead  camera.  It  can  be  seen  that  the  trajectories 
computed  using  bearing  only  cooperative  localization 
are  closer  to  the  trajectories  computed  using  the  over¬ 
head  camera.  The  estimation  error  plots  (X,  Y,  and 
ip)  of  the  blue  robot  are  shown  in  Fig.  15,  16,  and  17. 
It  can  be  seen  that  the  estimation  error  of  all  of  the 
states  using  bearing-only  cooperative  localization  with 
two  landmarks  is  bounded,  however,  the  estimation  er¬ 
ror  using  only  encoder  measurement  drifts. 

Additional  videos  of  simulation  and  experimental 
results  related  to  bearing-only  cooperative  localization 
can  be  found  in  [24], 


5  Conclusion 

In  this  paper,  we  develop  a  test-bed  of  three  ground 
robots,  which  are  equipped  with  wheel  encoders  and 
omnidirectional  cameras,  to  implement  the  bearing-only 
cooperative  localization.  The  simulation  and  experimen¬ 
tal  results  validate  the  observability  conditions,  derived 
in  [1] ,  for  the  complete  observability  of  the  bearing-only 
cooperative  localization  problem. 
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Fig.  15  Error  in  X  direction  of  the  blue  robot:The  dashed 
black  curve  represents  the  ±3crx  uncertainty.  The  estimation 
error  in  X  direction  using  encoder  measurements  is  repre¬ 
sented  by  the  red  solid  curve  with  diamonds,  and  the  esti¬ 
mation  in  X  direction  using  bearing-only  measurements  is 
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Fig.  17  Error  in  heading  of  the  blue  robot: The  dashed  black 
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red  solid  curve  with  diamonds,  and  the  estimation  error  in 
heading  using  bearing-only  measurements  is  represented  by 
the  blue  solid  curve  with  circles. 
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